/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Accelerometer;

/**
 *
 * @author gianmariospacagna
 */
public final class Position  {

    private double x;
    private double y;
    private double z;
    private double prevVelX;
    private double prevVelY;
    private double prevVelZ;
    private Velocity initVel;
    private long prevVelTime;
    private long time;

    public Position(Velocity initVel) {
        
        
        this.initVel=initVel;
        this.x=0;
        this.y=0;
        this.z=0;
        this.prevVelX=initVel.getX();
        this.prevVelY=initVel.getY();
        this.prevVelZ=initVel.getZ();
        this.prevVelTime=initVel.getTime();
        
    }
    
    public Position (){
        this.x=0;
        this.y=0;
        this.z=0;
        this.time=System.currentTimeMillis();
        
}
    public void updatePosition(Velocity vel){
        prevVelTime=time;
        time=vel.getTime();
        double deltaT=time-prevVelTime;
        deltaT/=1000;
        x+=vel.getX()*deltaT;
        y+=vel.getY()*deltaT;
        z+=vel.getZ()*deltaT;
        
        
    }

    public void updatePosition2(Velocity vel){
        double deltaT=vel.getTime()-prevVelTime;
        deltaT/=1000;
        x+=(vel.getX()+prevVelX-2*initVel.getX())*deltaT*0.5;
        y+=(vel.getY()+prevVelY-2*initVel.getY())*deltaT*0.5;
        z+=(vel.getZ()+prevVelZ-2*initVel.getZ())*deltaT*0.5;
        prevVelX=vel.getX();
        prevVelY=vel.getY();
        prevVelZ=vel.getZ();
        prevVelTime=vel.getTime();
    }

    public double getX() {
        return x;
    }

    public double getY() {
        return y;
    }

    public double getZ() {
        return z;
    }
    
    
    
}
